Imports System.Text
Imports System.ComponentModel
Imports System.Runtime.InteropServices

Public Class WingPort : Inherits Transport

#Region "protocal"
    Public Shared FrameGetVersion() As Byte = New Byte(4) {&H5A, &H0, &H0, &H5A, &H7E}
    Public Const ID_SET_EACH_MOTOR As Byte = &H8
    Public Const ID_SET_ARM As Byte = &H9
#End Region

    Public EachMotor As SetEachMotor_T
    Public Arm As SetHydroArm_T

    <Description("硬件版本")>
    Public Property Version As String

    Public Sub New()

    End Sub

    ''' <summary>
    ''' 下发电机控制指令到下位机
    ''' </summary>
    Public Sub SetEachMotor()
        SendMsg(GenerateMsg(MsgType.SetEachMotor, EachMotor))
        Dim string_v = AsyncReadUntil("~")
        Dim byte_v = Encoding.ASCII.GetBytes(string_v)
        Parse(byte_v)
    End Sub

    ''' <summary>
    ''' 控制左右抱臂状态
    ''' </summary>
    Public Sub SetArms()
        SendMsg(GenerateMsg(MsgType.SetHydroArm, Arm))
        Dim string_v = AsyncReadUntil("~")
    End Sub

    ''' <summary>
    ''' 尝试一下异步方法，其实这里没必要，因为ReadTo似乎是阻塞的
    ''' </summary>
    ''' <param name="finish"></param>
    ''' <returns></returns>
    Public Function AsyncReadUntil(finish As String) As String
        Dim ret As String = ""
        Console.WriteLine("wait ack")
        Try
            ret = port.ReadTo(finish)
        Catch ex As Exception
            Console.WriteLine($"[Error] {ex.Message}")
        End Try
        'Console.WriteLine(ret)
        Return ret
    End Function

    ''' <summary>
    ''' 请求硬件版本
    ''' </summary>
    ''' <returns></returns>
    Public Function AsyncRequestVersion() As String
        Dim string_v As String
        Dim byte_v As Byte()
        '发送请求帧
        port.Write(FrameGetVersion, 0, FrameGetVersion.Length)
        '读取应答
        string_v = AsyncReadUntil("~")
        byte_v = Encoding.ASCII.GetBytes(string_v)
        '协议解析
        Parse(byte_v)
        Return Me.Version
    End Function

    ''' <summary>
    ''' 解析协议
    ''' </summary>
    ''' <param name="frame"></param>
    ''' <returns>失败返回false</returns>
    Private Function Parse(frame As Byte()) As Boolean
        Dim ret As Boolean = True

        If frame.Length = 0 Then
            Return False
        End If

        If frame(0) <> &H5A Then
            ret = False
        Else
            Dim msg_id As Byte = frame(1)
            Dim len As Integer = frame(2)
            Select Case msg_id
                Case &H0
                    Dim _version(15) As Byte
                    Dim _date(15) As Byte
                    Array.Copy(frame, 3, _version, 0, _version.Length)
                    Array.Copy(frame, 3 + _version.Length, _date, 0, _date.Length)
                    Me.Version = Encoding.ASCII.GetString(_version).Replace(vbNullChar, "") + "-" + Encoding.ASCII.GetString(_date).Replace(vbNullChar, "")
                    Console.WriteLine($"version: {Version}")
                Case ID_SET_EACH_MOTOR
                    Console.WriteLine("motor control success.")
                Case Else
            End Select
        End If
        Return ret
    End Function

    ''' <summary>
    ''' 生成要发送的协议
    ''' </summary>
    ''' <param name="msg_id">信息类型</param>
    ''' <param name="data_struct">数据结构体</param>
    ''' <returns></returns>
    Public Function GenerateMsg(msg_id As MsgType, data_struct As Object) As Byte()
        Dim tx_buffer(4) As Byte
        Dim data_buf() As Byte = Nothing
        Dim data_len As Integer = 0
        tx_buffer(0) = &H5A

        data_len = Marshal.SizeOf(data_struct)
        data_buf = Transport.StructToByte(data_struct)
        tx_buffer(2) = data_len
        ReDim Preserve tx_buffer(data_len + 4)

        If msg_id = MsgType.SetHydroArm Then
            tx_buffer(1) = ID_SET_ARM
        ElseIf msg_id = MsgType.SetEachMotor Then
            tx_buffer(1) = ID_SET_EACH_MOTOR
        End If

        If data_buf IsNot Nothing Then
            Array.Copy(data_buf, 0, tx_buffer, 3, data_buf.Length)
        End If
        '填入固定结束符
        Dim crc As Byte = Calculate.Sum(tx_buffer)
        tx_buffer(tx_buffer.Length - 2) = crc
        tx_buffer(tx_buffer.Length - 1) = &H7E
        Return tx_buffer
    End Function

    ''' <summary>
    ''' 下发协议
    ''' </summary>
    ''' <param name="buf">协议内容</param>
    ''' <returns></returns>
    Public Function SendMsg(buf() As Byte) As Boolean
        Dim ret As Boolean = True
        If buf.Length < 5 Then
            ret = False
        Else
            If buf(0) <> &H5A Then
                ret = False
            Else
                Dim total_len As Integer = buf(2) + 5
                port.Write(buf, 0, total_len)
            End If
        End If
        Return ret
    End Function

End Class

Public Class Calculate
    Shared Function Sum(ByVal buf() As Byte) As Byte
        Dim ret As Byte = 0
        Dim _sum As Integer = 0
        For Each b As Byte In buf
            _sum = (_sum + b) Mod 256
        Next
        ret = _sum
        Return ret
    End Function
End Class

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetHydroArm_T

    Public Structure Couple
        Public Structure Arm
            Public Strech As Byte
            Public DrawBack As Byte
            Public Pump As Byte
        End Structure

        Public ArmL As Arm
        Public ArmR As Arm
    End Structure

    Public Couples As Couple

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetEachMotor_T

    Public Structure Wheel
        Public VL As Short
        Public VR As Short
    End Structure

    Public WheelFL As Wheel
    Public WheelFR As Wheel
    Public WheelBL As Wheel
    Public WheelBR As Wheel

End Structure

Public Enum MsgType
    SetHydroArm
    SetEachMotor
End Enum
